/*
 * Avoibot32.c
 *
 * Created: 13-03-2013 23:55:16
 *  Author: Karmu
 */ 


//#define F_CPU 11059200UL			/* 11.0592 MHz crystal clock */ //low fuse EF
//#define F_CPU 1000000UL			/* 1 MHz internal RC clock */
#define F_CPU 8000000UL			    /* 8 MHz internal RC clock */ //low fuse E4
#include <avr/io.h>
#include <avr/interrupt.h>   
#include <util/delay.h>
#include <avr/pgmspace.h>
#include "lcd.h"  // Using Peter Fleury's LCD library
#include <stdlib.h>

#define DELAY		1000

/* defines for LCD (other defines are in LCD.h) */
#define LCDDELAY	200

/* defines for motor control */
#define MOTOR_PORT	PORTD
#define MOTOR_DDR	DDRD

#define M_L_PORT	MOTOR_PORT
#define M_L_DDR		MOTOR_DDR
#define M_L_IN1		PD6
#define M_L_IN2		PD7

#define M_R_PORT	MOTOR_PORT
#define M_R_DDR		MOTOR_DDR
#define M_R_IN1		PD1			//IN3
#define M_R_IN2		PD0			//IN4

#define M_PWM_PORT	MOTOR_PORT
#define M_PWM_DDR	MOTOR_DDR
#define M_L_PWM		PD5
#define M_R_PWM		PD4

#define MOTOR_L		1
#define MOTOR_R		2

#define M_FORWARD	0
#define M_REVERSE	1
#define M_STOP		2

#define GO_LEFT		0
#define GO_HARD_LEFT 1
#define GO_FORWARD	2
#define GO_REVERSE	3
#define GO_HARD_RIGHT 4
#define GO_RIGHT	5
#define GO_BRAKE	6

/* define PWM for Motor Speed control */
/* define TOP & BOTTOM for PWM (ICR1) */
#define TOP			10000 //8 MHz 1 pre-scalar
#define BOTTOM		3000
//#define TOP		16000 //16Mhz 265 pre-scalar
//#define MINIMUM_DUTY	5000
#define	STEP_SIZE	10

/* defines for IR sensors */
#define IR_DDR DDRB          /* DDRx - register for IR output */
#define IR_PORT PORTB        /* PORTx -register for IR output */
#define IR_PIN PINB          /* PINx - register for IR input  */
#define IR_BIT_LEFT PB0      /* bit for IR input/output */
#define IR_BIT_RIGHT PB1     /* bit for IR input/output */
#define IR_BIT_CENTER PB2    /* bit for IR input/output */
#define IR_BIT_REAR PB3      /* bit for IR input/output */
#define ON 1
#define OFF 0

/*
** constant definitions
*/
static const PROGMEM unsigned char copyRightChar[] =
{
	0x07, 0x08, 0x13, 0x14, 0x14, 0x13, 0x08, 0x07,
	0x00, 0x10, 0x08, 0x08, 0x08, 0x08, 0x10, 0x00
};
static const PROGMEM unsigned char blockChar[] =
{
	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF
};


/* function prototypes */
void io_init();
void init_pwm();
void set_motor_direction(uint8_t motor, uint8_t direction);
void steer_robot(uint8_t direction);
void booting();
void encoder_display();
void steer_display(const char *Direction);
void sensor_display(const char *Text);

int
main(void)
{
	/* variable declarations */
	uint8_t last_direction = GO_FORWARD;    /* last direction steered */
	uint8_t left,right,center,rear;

	/* initializations */
	io_init();

	/* initialize LCD display, cursor off */
	lcd_init(LCD_DISP_ON);
	lcd_clrscr();   /* clear display home cursor */
	
	/* Booting animation */
	booting();
	
	/* Start the Robot Forward */
	steer_robot(GO_FORWARD);
	
	/* Main loop */
	while(1)
	{
		OCR1A = OCR1A+1; //left
		OCR1B = OCR1B+1; //right
		if (OCR1A>TOP) OCR1A=BOTTOM;
		if (OCR1B>TOP) OCR1B=BOTTOM;
		//encoder_display();
		//_delay_ms(DELAY);
		//sensor_display("Waiting         ");
		if (bit_is_set(IR_PIN, IR_BIT_LEFT)) {left=ON;sensor_display("Left            ");}
		else left=OFF;
					
		if (bit_is_set(IR_PIN, IR_BIT_CENTER)) {center=ON;sensor_display("Center           ");}
		else center=OFF;
					
		if (bit_is_set(IR_PIN, IR_BIT_RIGHT)) {right=ON;sensor_display("Right           ");}
		else right=OFF;
					
		if (bit_is_set(IR_PIN, IR_BIT_REAR)) {rear=ON;sensor_display("Rear            ");}
		else rear=OFF;

		if (rear==ON)
		{
			//HALT
			//steer_robot(GO_BRAKE);
			//_delay_ms(DELAY*3);
			steer_robot(GO_FORWARD);
			_delay_ms(DELAY/2);
			last_direction = GO_FORWARD;
		}
		else if ((left==OFF) && (right==OFF) && (center==OFF))
		{
			//MARCH FORWARD
			sensor_display("Waiting         ");
			steer_robot(GO_FORWARD);
			last_direction = GO_FORWARD;
		}
		else if (left==ON && right==ON && center==ON)
		{
			//REVERSE N RIGHT
			steer_robot(GO_BRAKE);
			_delay_ms(DELAY);
			steer_robot(GO_REVERSE);
			_delay_ms(DELAY);
			steer_robot(GO_HARD_LEFT);
			_delay_ms(DELAY);
			last_direction = GO_HARD_LEFT;
						
		}
		else if (left==OFF && right==OFF && center==ON)
		{
			//REVERSE N LEFT
			steer_robot(GO_BRAKE);
			_delay_ms(DELAY);
			steer_robot(GO_REVERSE);
			_delay_ms(DELAY);
			steer_robot(GO_HARD_LEFT);
			_delay_ms(DELAY);
			last_direction = GO_HARD_LEFT;
		}
		else if (left==ON && right==OFF && center==OFF)
		{
			//RIGHT TURN
			//steer_robot(GO_RIGHT);
			//last_direction = GO_RIGHT;
			steer_robot(GO_HARD_RIGHT);
			last_direction = GO_HARD_RIGHT;
		}
		else if (left==OFF && right==ON && center==OFF)
		{
			//LEFT TURN
			//steer_robot(GO_LEFT);
			//last_direction = GO_LEFT;
			steer_robot(GO_HARD_LEFT);
			last_direction = GO_HARD_LEFT;
		}
		else if (left==ON && right==OFF && center==ON)
		{
			//TURN HARD RIGHT
			steer_robot(GO_HARD_RIGHT);
			last_direction = GO_HARD_RIGHT;
		}
		else if (left==OFF && right==ON && center==ON)
		{
			//TURN HARD LEFT
			steer_robot(GO_HARD_LEFT);
			last_direction = GO_HARD_LEFT;
		}
		else
		{
			//If the robot does not see a obstacle at all Continue going the direction last it was moving to.
			steer_robot(last_direction);
		}
	}
}

void
booting()
{
	int i;
	lcd_puts("-- Booting... --\n");
	/*
    * load two user defined characters from program memory
    * into LCD controller CG RAM location 0 and 1
    */
	lcd_command(_BV(LCD_CGRAM));  /* set CG RAM start address 0 */
	for(i=0; i<16; i++)
	{
		lcd_data(pgm_read_byte_near(&blockChar[i]));
	}
	for(i=0; i<16; i++)
    {
		/* create illusion of progress bar */
		_delay_ms(LCDDELAY);
		/* increment cursor to position i on line 2 */
        lcd_gotoxy(i,1);  
        /* write single char to display */
        lcd_putc(0);
	}	
	
	lcd_clrscr();
	lcd_puts("Avoibot Running\n");
}

void 
io_init()
{
	/* setup input/outputs */
	M_L_DDR |= _BV(M_L_IN1) | _BV(M_L_IN2) ;
	M_R_DDR |= _BV(M_R_IN1) | _BV(M_R_IN2) ;

	M_PWM_DDR |= _BV(M_L_PWM) | _BV(M_R_PWM);

	IR_DDR &= ~_BV(IR_BIT_LEFT);		/* Pin as input */
	//IR_PORT |= _BV(IR_BIT_LEFT);		/* Enable internal pull-up */

	IR_DDR &= ~_BV(IR_BIT_RIGHT);		/* Pin as input */
	//IR_PORT |= _BV(IR_BIT_RIGHT);		/* Enable internal pull-up */
		
	IR_DDR &= ~_BV(IR_BIT_CENTER);		/* Pin as input */
	//IR_PORT |= _BV(IR_BIT_CENTER);	/* Enable internal pull-up */
		
	IR_DDR &= ~_BV(IR_BIT_REAR);		/* Pin as input */
	//IR_PORT |= _BV(IR_BIT_REAR);	    /* Enable internal pull-up */
		
	//ENCODER_DDR &= ~_BV(ENCODER_L);		/* Pin as input */
	//ENCODER_PORT |= _BV(ENCODER_L);	/* Enable internal pull-up */
		
	//ENCODER_DDR &= ~_BV(ENCODER_R);		/* Pin as input */
	//ENCODER_PORT |= _BV(ENCODER_R);	/* Enable internal pull-up */
		
	/* set INT0 and INT1 interrupt enable bit */
	GICR |= _BV(INT0) | _BV(INT1);
		
	/* The rising edge of INT0 and INT1 generates an interrupt request. */
	MCUCR |= _BV(ISC11) | _BV(ISC10) | _BV(ISC01) | _BV(ISC00);
	
	/* enable interrupts */
	sei();
}

void
init_pwm()
{
	/* setup phase corrected PWM for motor*/
	/* Set OC1A/OC1B on Compare Match when up-counting.
	Clear OC1A/OC1B on Compare Match when down-counting. */
	TCCR1A |= _BV(COM1A1) | _BV(COM1B1); // | _BV(COM1A0) | _BV(COM1B0);
	
	/* Phase Correct and Phase and Frequency Correct PWM */
	TCCR1B |= _BV(WGM13);
	
	/* Pre-scalar 1 */
	TCCR1B |= _BV(CS10);
	
	/* set the TOP value ICR1 */
	ICR1 = TOP;
	
	/* Start with 30% PWM */
	OCR1A = BOTTOM;
}


void
steer_display(const char *Direction)
{
	/*Resetting line 2*/
	lcd_gotoxy(0,1);
	//lcd_puts("                ");
	lcd_puts(Direction);
}
void
sensor_display(const char *Text)
{
	/*Resetting line 2*/
	lcd_gotoxy(0,0);
	//lcd_puts("                ");
	lcd_puts(Text);
}


void 
set_motor_direction(uint8_t motor, uint8_t direction)
{
	if (motor==1)
	{
		switch (direction)
			{
					case M_REVERSE:
						M_L_PORT |=  _BV(M_L_IN2);		/* set DIR pin */
		                M_L_PORT &= ~_BV(M_L_IN1);		/* clear DIR pin */ 
						break;
					case M_FORWARD:
		                M_L_PORT &= ~_BV(M_L_IN2);		/* clear DIR pin */ 
						M_L_PORT |=  _BV(M_L_IN1);		/* set DIR pin */
						break;
					case M_STOP:
						M_L_PORT &= ~_BV(M_L_IN2);		/* clear DIR pin */
						M_L_PORT &= ~_BV(M_L_IN1);		/* clear DIR pin */
						/* Alternatively */
						//M_L_PORT |=  _BV(M_L_IN1);		/* set DIR pin */
						//M_L_PORT |=  _BV(M_L_IN2);		/* set DIR pin */
						break;				
			}
	}
	else
	{
		switch (direction)
			{
					case M_REVERSE:
						M_R_PORT |=  _BV(M_R_IN2);		/* set DIR pin */
		                M_R_PORT &= ~_BV(M_R_IN1);		/* clear DIR pin */ 
						break;
					case M_FORWARD:
		                M_R_PORT &= ~_BV(M_R_IN2);		/* clear DIR pin */ 
						M_R_PORT |=  _BV(M_R_IN1);		/* set DIR pin */
						break;
					case M_STOP:
						M_R_PORT &= ~_BV(M_R_IN2);		/* clear DIR pin */
						M_R_PORT &= ~_BV(M_R_IN1);		/* clear DIR pin */
						//M_R_PORT |=  _BV(M_R_IN1);		/* set DIR pin */
						//M_R_PORT |=  _BV(M_R_IN2);		/* set DIR pin */
						break;				
			}		
	}
}

void steer_robot(uint8_t direction)
{
        
        switch (direction)
        {
                case GO_LEFT:
                        set_motor_direction(MOTOR_L, M_FORWARD);
                        set_motor_direction(MOTOR_R, M_STOP);
						steer_display("LEFT TURN        ");
                        break;
                
                case GO_HARD_LEFT:
                        set_motor_direction(MOTOR_L, M_FORWARD);
                        set_motor_direction(MOTOR_R, M_REVERSE);
						steer_display("LEFT HARD TURN   ");
                        break;
                
                case GO_FORWARD:
                        set_motor_direction(MOTOR_L, M_FORWARD);
                        set_motor_direction(MOTOR_R, M_FORWARD);
						steer_display("MARCH FORWARD    ");
                        break;
						
                case GO_REVERSE:
                        set_motor_direction(MOTOR_L, M_REVERSE);
                        set_motor_direction(MOTOR_R, M_REVERSE);
						steer_display("REVERSE          ");
                        break;
						
                case GO_HARD_RIGHT:
                        set_motor_direction(MOTOR_L, M_REVERSE);
                        set_motor_direction(MOTOR_R, M_FORWARD);
						steer_display("RIGHT HARD TURN  ");
                        break;
                
                case GO_RIGHT:
                        set_motor_direction(MOTOR_L, M_STOP);
                        set_motor_direction(MOTOR_R, M_FORWARD);
						steer_display("RIGHT TURN       ");
                        break;
                
                case GO_BRAKE:
                        set_motor_direction(MOTOR_L, M_STOP);
                        set_motor_direction(MOTOR_R, M_STOP);
						steer_display("STOPED          ");
                        break;
                break;    
        }
}